# coding=utf-8
import numpy as np  # type: ignore
from scipy.optimize import linear_sum_assignment
import rclpy
from rclpy.node import Node
from rclpy.timer import Timer
import rclpy.qos
from rcl_interfaces.srv import GetParameters
from geometry_msgs.msg import Twist, PoseStamped, Pose
from scipy.spatial.transform import Rotation as R  # type: ignore
from std_msgs.msg import Float64MultiArray, MultiArrayDimension
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
from laser_geometry import LaserProjection
from sensor_msgs.msg import NavSatFix, LaserScan, PointCloud2, Image
import sys, select, os
import tty, termios
import signal
import time
import math
import socket, struct
from scipy.spatial import cKDTree
from interfaces_consensus.msg import CommandMsg
from builtin_interfaces.msg import Time
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Point, Quaternion


EARTH_RADIUS = 6371008.7714150598

def norm(pos1, pos2):
    A_np = np.array(pos1)
    B_np = np.array(pos2)
    distance = np.linalg.norm(A_np - B_np)
    return distance


class consensus_APF_uav(Node):
    def __init__(self):
        # 初始化节点，指定节点名称
        super().__init__('consenses_APF_new')
        # self.folnum = int(sys.argv[1])  # 无人机数量
        self.folnum = 1
        self.uav_id = int(sys.argv[1])  #无人机编号
        
        # UDP多播发送
        MCAST_GRP = '224.1.11.111'
        MCAST_PORT = 5007
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
        self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 32)

        self.clusterId = 0 #集群编号
        self.cluster_list = list([self.uav_id]) #集群组成列表
        self.childClusterId = 0  #子集群编号
        self.childCluster_list = [] #子集群组成列表
        self.camp = 0 #阵营 0：红方 1：蓝方
        self.detect_R = 50.0  # 障碍物检测范围
        self.task_goal = [None, None, None]  # 无人机任务的部署坐标
        self.goal = [0.0, 0.0, 0.0]  # 编队飞行的目标点坐标
        self.formation = 0  # 队形
        self.interval = 50.0  #队形间距
        self.safe_distance = 30.0 #安全距离
        self.cluster_veclocity = 3.0 #集群最大速度
        self.status = "init"  #集群控制器状态
        self.original_position = [0.0, 0.0, 0.0]
        self.A = self.generate_A(self.folnum+1,1)  #初始化邻接矩阵

        #最小距离
        self.min_distance = 10000.0

        # 返航位置
        self.return_position = np.zeros((self.folnum,3),dtype=np.float64)
        self.filtered_obstacles = []  # 初始化filtered_obstacles
        self.calculated_obstacles = []  # 初始化calculated_obstacles
        self.colony_list = list([self.uav_id])  # 编队成员执行列表    
        # print(f"colony_list={self.colony_list}")

        self.cluster_flag = False  #初始化集群指令接收标志
        self.first_init_flag = False #是否初始化标志
        self.return_to_home_flag = False #返航指令接收标志，更改邻接矩阵
        self.arrive_flag = [False] * self.folnum #各无人机到达目标点标志
        self.task_exec_flag = [False] * self.folnum
        self.status_return_flag = False
        self.task_planning_flag = False

        self.longitude = [0.0] * self.folnum
        self.latitude = [0.0] * self.folnum
        self.altitude = [0.0] * self.folnum
        self.uavpos_x = [0.0] * self.folnum
        self.uavpos_y = [0.0] * self.folnum
        self.uavpos_z = [0.0] * self.folnum
        self.uav_velocity_x = [0.0] * self.folnum
        self.uav_velocity_y = [0.0] * self.folnum
        self.uav_velocity_z = [0.0] * self.folnum

        self.cluster_longitude = [0.0] * self.folnum
        self.cluster_latitude = [0.0] * self.folnum
        self.cluster_altitude = [0.0] * self.folnum
        self.cluster_uavpos_x = [0.0] * self.folnum
        self.cluster_uavpos_y = [0.0] * self.folnum
        self.cluster_uavpos_z = [0.0] * self.folnum
        self.cluster_timestamps = [None] * self.folnum

        # 队形相对位置定义 (领导者与跟随者之间的相对位置)
        self.delta_x = np.zeros((self.folnum+1),dtype=int) # x方向
        self.delta_y = np.zeros((self.folnum+1),dtype=int) # y方向
        self.delta_z = np.zeros((self.folnum+1),dtype=int)

        self.init_f = np.zeros((self.folnum + 1, 4))  # 初始化8个无人机和虚拟leader的坐标
        self.init_v = np.zeros((self.folnum + 1, 3))  #初始化8个无人机和虚拟leader的速度
        self.timestamps = [None] * self.folnum  # 初始化8个时间戳列表（假设8个无人机，0号无人机不需要）

        # 动态生成 Pose 对象列表，每个 Pose 对象存储无人机的当前位置
        self.poses = [Float64MultiArray() for _ in range(self.folnum)]
        # self.uav_pose_pubs = {}

        ##线程分群赋值
        # group1 = MutuallyExclusiveCallbackGroup()
        # group2 = MutuallyExclusiveCallbackGroup()

        qos_profile_1 = rclpy.qos.QoSProfile(
            reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
            history=rclpy.qos.HistoryPolicy.KEEP_LAST,
            depth=10
        )
        qos_profile_2 = rclpy.qos.QoSProfile(
            reliability=rclpy.qos.ReliabilityPolicy.RELIABLE,
            history=rclpy.qos.HistoryPolicy.KEEP_LAST,
            depth=10
        )
        # for i in range(self.folnum):
        #     index = i + 1
        #     topic = "/drone{}/setpoint_raw/local".format(index)
        #     if i not in self.uav_pose_pubs:
        #         self.uav_pose_pubs[i] = self.create_publisher(Float64MultiArray, topic, 10)
        topic_plan_cmd_pub = "/drone{}/planning/command".format(self.uav_id)
        self.plan_cmd_pub = self.create_publisher(Float64MultiArray, topic_plan_cmd_pub, 10)
        topic_ori_pose_pub = "/drone{}/original_position".format(self.uav_id)
        self.ori_pose_pub = self.create_publisher(Float64MultiArray, topic_ori_pose_pub, 10)

        # # 监听激光雷达点云数据
        # self.vehicle_scan_subscriber = self.create_subscription(
        #         LaserScan,'/scan', self.vehicle_scan_callback, 10)
        # #转换Laser数据到PointCloud2
        # self.laserProj = LaserProjection()
        # self.laser_to_pointcloud_publisher = self.create_publisher(
        #         PointCloud2, '/scan_pcl2', 10)
        cmd_topic = "/drone{}/consensus_APF/command".format(self.uav_id)
        self.subscription = self.create_subscription(
            CommandMsg,  # 消息类型
            cmd_topic,  # 话题名称
            self.consensus_APF_command_callback,  # 回调函数
            qos_profile_2
        )
        # 接收障碍物点云参数
        # self.client = self.create_client(GetParameters,
        #                                  f'/drone{self.uav_id}/point_cloud_extractor/get_parameters')
        self.client = self.create_client(GetParameters,
                                         f'/drone{self.uav_id}/point_cloud_extractor/get_parameters')
        request = GetParameters.Request()
        request.names = ['calculated_obstacles']

        self.client.wait_for_service()

        future = self.client.call_async(request)
        future.add_done_callback(self.callback_global_param)
        # 凌云
        # future.add_done_callback(self.callback_global_param_from_goal_node)

        #         self.prometheus_vehicle_status_subscriber = self.create_subscription(
        # DroneState, '/'+node_name+'/prometheus/drone_state', self.prometheus_vehicle_status_callback, 10)

        self.drones_pose_sub = []
        for i in self.colony_list:  # 创建话题名称
            topic_name = "/drone{}/velocity_and_position".format(i)
            self.get_logger().info(f"Listen to {topic_name}")

            drone_pose_sub = self.create_subscription(
                Float64MultiArray,
                topic_name,
                lambda msg, id=i: self.pose_callback(msg, id),
                10
            )
            self.drones_pose_sub.append(drone_pose_sub)

        self.timer1 = self.create_timer(0.01, self.execAPFcallback)
        self.timer2 = self.create_timer(2.0, self.checkObstaclescallback)
        # self.timer3 = self.create_timer(2.0, self.checkUavPos)

        # 打印启动日志
        self.get_logger().info("'consenses_APF_new' node has been started!")
        self.get_logger().info("Wait for initialization ...")

    def sendStatus(self,status):
        ''' 0:初始化成功 
            1:重规划成功
            2:接收到指令'''
        if status == 0:
            type = b'\x25'
            head = b'\x04'
            status_data = struct.pack(
                    '=2c2i',
                    type, head, status, self.uav_id)
            # print(len(status_data))
            # print(status_data)
            self.sock.sendto(status_data, (f'224.1.11.111', 5007))
        else:
            type = b'\x05'
            head = b'\x05'
            status_data = struct.pack(
                    '=2c4i',
                    type, head, self.clusterId, self.childClusterId, self.uav_id, status)
            # print(len(status_data))
            # print(status_data)
            self.sock.sendto(status_data, (f'224.1.11.111', 5007))

    def execAPFcallback(self):
        if self.status == "init":
            if self.cluster_flag and all(timestamp is not None for timestamp in self.timestamps):
                # self.flag = True #与凌云调试
                self.first_init_flag = True
                self.original_position[0] = self.longitude[0]
                self.original_position[1] = self.latitude[0]
                self.original_position[2] = self.altitude[0]
                for index in range(self.folnum):
                    self.return_position[index][0] = self.longitude[index]
                    self.return_position[index][1] = self.latitude[index]
                    self.return_position[index][2] = self.altitude[index]
                self.get_logger().info(
                    f"get original lon: {self.original_position[0]}--lat: {self.original_position[1]}--alt: {self.original_position[2]}")
                # 用来储存算法中k的值
                self.K = 0
                # 将位置数据存入位置矩阵的对应行
                for drone_id in range(self.folnum):
                    x = (self.longitude[drone_id] - self.original_position[0]) / 180 * math.pi * (
                                EARTH_RADIUS + self.original_position[2])
                    y = (self.latitude[drone_id] - self.original_position[1]) / 180 * math.pi * (
                                EARTH_RADIUS + self.original_position[2])
                    z = self.altitude[drone_id] - self.original_position[2]
                    print(f"drone{self.colony_list[drone_id]} x:{x} y: {y} z:{z}")
                    self.init_f[drone_id] = [x, y, z, 0]
                    v_x = self.uav_velocity_x[drone_id]
                    v_y = self.uav_velocity_y[drone_id]
                    v_z = self.uav_velocity_z[drone_id]
                    self.init_v[drone_id] = [v_x, v_y, v_z]
                    self.delta_x[drone_id] = x
                    self.delta_y[drone_id] = y
                    self.delta_z[drone_id] = z
                self.init_f[self.folnum] = [sum(self.delta_x[:-1])/self.folnum, sum(self.delta_y[:-1])/self.folnum, sum(self.delta_z[:-1])/self.folnum, 0]
                self.init_v[self.folnum] = [0.0, 0.0, 0.0]
                self.delta_x[self.folnum] = self.init_f[self.folnum][0]
                self.delta_y[self.folnum] = self.init_f[self.folnum][1]
                self.delta_z[self.folnum] = self.init_f[self.folnum][2]
                self.get_logger().info("Initialization successful !!")
                self.sendStatus(0)
                self.status = "waittarget"
                self.get_logger().info("Waiting target ...")
            
        elif self.status == "waittarget":
            # self.get_logger().info("Waiting target ...")
            # if self.task_planning_flag:
            #     self.status = "task"
            return

        elif self.status == "return_to_home":
            self.colony_list = self.cluster_list
            self.formation = 4
            return_lon = np.mean(self.return_position[:,0])
            return_lat = np.mean(self.return_position[:,1])
            return_alt = np.mean(self.return_position[:,2])
            self.goal[0] = (return_lon - self.original_position[0]) / 180 * math.pi * (
                    EARTH_RADIUS + self.original_position[2])
            self.goal[1] = (return_lat - self.original_position[1]) / 180 * math.pi * (
                    EARTH_RADIUS + self.original_position[2])
            self.goal[2] = return_alt - self.original_position[2]


            self.return_to_home_flag = True 

            self.get_logger().info("return to home ...")
            self.status = "grouping"

        elif self.status == "gentraj":
            # self.sendStatus(3)
            self.get_logger().info("Generate tracks ...")
            # 记录开始时间
            start_time = time.time()
            self.perform_consistency_algorithm()
            self.K = 0
            # 记录结束时间
            end_time = time.time()
            # 计算并打印执行时间
            execution_time = end_time - start_time
            self.get_logger().info(f"程序执行时间：{execution_time:.4f} 秒")
            self.sendStatus(1)

            self.status = "exec"
        elif self.status == "exec":
           # 发送初始点
            if self.original_position[0] == 0.0 and self.original_position[1] == 0.0 and self.original_position[2] == 0.0:
                return
            ori_pose_msg = Float64MultiArray()
            ori_pose_msg.data = self.original_position
            self.ori_pose_pub.publish(ori_pose_msg)
            self.get_logger().info(f'Publishing original position: {self.original_position}')

            # 发送规划控制点
            rows, cols = self.traj_pose_x.shape
            # 将每个矩阵展平并合并
            data = np.concatenate([
                self.traj_V_x.flatten(),
                self.traj_V_y.flatten(),
                self.traj_V_z.flatten(),
                self.traj_pose_x.flatten(),
                self.traj_pose_y.flatten(),
                self.traj_pose_z.flatten()
            ])

            # 创建 Float64MultiArray 消息
            pose_msg = Float64MultiArray()
            pose_msg.data = data.tolist()

            # 设置 layout，指定行列信息
            pose_msg.layout.dim = [
                MultiArrayDimension(label='row', size=rows, stride=cols * 6),  # 6 个矩阵，stride 是每个矩阵的列数
                MultiArrayDimension(label='column', size=cols, stride=1)
            ]

            self.plan_cmd_pub.publish(pose_msg)
            self.get_logger().info('Publishing planning data')
            self.status = "waittarget"
            self.get_logger().info("Waiting target ...")

        elif self.status == "replan":
            if all(timestamp is not None for timestamp in self.timestamps):
                self.get_logger().info("Replanning ...")
                # 将位置数据存入位置矩阵的对应行
                for drone_id in range(self.folnum):
                    x = (self.longitude[drone_id] - self.original_position[0]) / 180 * math.pi * (
                                EARTH_RADIUS + self.original_position[2])
                    y = (self.latitude[drone_id] - self.original_position[1]) / 180 * math.pi * (
                                EARTH_RADIUS + self.original_position[2])
                    z = self.altitude[drone_id] - self.original_position[2]
                    print(f"drone{self.colony_list[drone_id]} x:{x} y: {y} z:{z}")
                    self.init_f[drone_id] = [x, y, z, 0]
                    v_x = self.uav_velocity_x[drone_id]
                    v_y = self.uav_velocity_y[drone_id]
                    v_z = self.uav_velocity_z[drone_id]
                    self.init_v[drone_id] = [v_x, v_y, v_z]
                    self.delta_x[drone_id] = x
                    self.delta_y[drone_id] = y
                    self.delta_z[drone_id] = z
                    # 凌云
                    # x = self.traj_pose_x[drone_id, self.traj_k]
                    # y = self.traj_pose_y[drone_id, self.traj_k]
                    # z = self.traj_pose_z[drone_id, self.traj_k]
                    # self.init_f[drone_id] = [x, y, z, 0]
                    # self.delta_x[drone_id] = x
                    # self.delta_y[drone_id] = y
                    # self.delta_z[drone_id] = z
                # 将编队中心点位置赋值给虚拟leader
                # x = self.delta_x[0]
                # y = self.delta_y[0]
                # z = self.delta_z[0]
                v_x = self.uav_velocity_x[0]
                v_y = self.uav_velocity_y[0]
                v_z = self.uav_velocity_z[0]
                # self.init_f[self.folnum] = [x, y, z, 0]
                self.init_f[self.folnum] = [sum(self.delta_x[:-1])/self.folnum, sum(self.delta_y[:-1])/self.folnum, sum(self.delta_z[:-1])/self.folnum, 0]
                self.init_v[self.folnum] = [v_x, v_y, v_z]
                self.delta_x[self.folnum] = self.init_f[self.folnum][0]
                self.delta_y[self.folnum] = self.init_f[self.folnum][1]
                self.delta_z[self.folnum] = self.init_f[self.folnum][2]

                if self.return_to_home_flag:
                    self.A = self.generate_A(self.folnum+1,0)
                    self.return_to_home_flag = False
                else:
                    self.A = self.generate_A(self.folnum+1,1)
                
                # self.sendStatus(3)
                self.get_logger().info("Generate tracks ...")
                # 记录开始时间
                start_time = time.time()
                self.perform_consistency_algorithm()
                self.K = 0
                # 记录结束时间
                end_time = time.time()
                # 计算并打印执行时间
                execution_time = end_time - start_time
                self.get_logger().info(f"程序执行时间：{execution_time:.4f} 秒")
                self.sendStatus(1)

                # 发送初始点
                if self.original_position[0] == 0.0 and self.original_position[1] == 0.0 and self.original_position[2] == 0.0:
                    return
                ori_pose_msg = Float64MultiArray()
                ori_pose_msg.data = self.original_position
                self.ori_pose_pub.publish(ori_pose_msg)
                self.get_logger().info(f'Publishing original position: {self.original_position}')

                # 发送规划控制点
                rows, cols = self.traj_pose_x.shape
                # 将每个矩阵展平并合并
                data = np.concatenate([
                    self.traj_V_x.flatten(),
                    self.traj_V_y.flatten(),
                    self.traj_V_z.flatten(),
                    self.traj_pose_x.flatten(),
                    self.traj_pose_y.flatten(),
                    self.traj_pose_z.flatten()
                ])

                # 创建 Float64MultiArray 消息
                pose_msg = Float64MultiArray()
                pose_msg.data = data.tolist()

                # 设置 layout，指定行列信息
                pose_msg.layout.dim = [
                    MultiArrayDimension(label='row', size=rows, stride=cols * 6),  # 6 个矩阵，stride 是每个矩阵的列数
                    MultiArrayDimension(label='column', size=cols, stride=1)
                ]

                self.plan_cmd_pub.publish(pose_msg)
                self.get_logger().info('Publishing planning data')
                self.status = "waittarget"
                self.get_logger().info("Waiting target ...")
            else:
                self.get_logger().info("Wait for replanning ...")
        elif self.status == "task":
            self.colony_list = list([self.uav_id])
            self.goal = self.task_goal
            self.get_logger().info(
                f'Execute the task, goal:{self.goal}')
            self.status="grouping"
        
        elif self.status == "grouping":
            self.folnum = len(self.colony_list)
            self.arrive_flag = [False] * self.folnum
            self.timestamps = [None] * self.folnum  # 初始化8个时间戳列表（假设8个无人机，0号无人机不需要）

            self.longitude = [0.0] * self.folnum
            self.latitude = [0.0] * self.folnum
            self.altitude = [0.0] * self.folnum
            self.uavpos_x = [0.0] * self.folnum
            self.uavpos_y = [0.0] * self.folnum
            self.uavpos_z = [0.0] * self.folnum
            self.uav_velocity_x = [0.0] * self.folnum
            self.uav_velocity_y = [0.0] * self.folnum
            self.uav_velocity_z = [0.0] * self.folnum
            
            self.cluster_longitude = [0.0] * len(self.cluster_list)
            self.cluster_latitude = [0.0] * len(self.cluster_list)
            self.cluster_altitude = [0.0] * len(self.cluster_list)
            self.cluster_uavpos_x = [0.0] * len(self.cluster_list)
            self.cluster_uavpos_y = [0.0] * len(self.cluster_list)
            self.cluster_uavpos_z = [0.0] * len(self.cluster_list)
            self.cluster_timestamps = [None] * len(self.cluster_list)

            # 队形相对位置定义 (领导者与跟随者之间的相对位置)
            self.delta_x = np.zeros((self.folnum+1),dtype=int) # x方向
            self.delta_y = np.zeros((self.folnum+1),dtype=int) # y方向
            self.delta_z = np.zeros((self.folnum+1),dtype=int)
            
            self.init_f = np.zeros((self.folnum + 1, 4))  # 初始化无人机和虚拟leader的坐标
            self.init_v = np.zeros((self.folnum + 1, 3))

            # 动态生成 Pose 对象列表，每个 Pose 对象存储无人机的当前位置
            self.poses = [Float64MultiArray() for _ in range(self.folnum)]
            

            self.update_subscriptions(self.cluster_list)
            if self.first_init_flag:
                self.status = "replan"
            else:
                self.return_position = np.zeros((self.folnum,3),dtype=np.float64)
                self.status = "init"

        elif self.status == "stop":
            self.traj_V_x = np.zeros((self.folnum + 1, 1))
            self.traj_V_y = np.zeros((self.folnum + 1, 1))
            self.traj_V_z = np.zeros((self.folnum + 1, 1))
            self.traj_pose_x = np.zeros((self.folnum + 1, 1))
            self.traj_pose_y = np.zeros((self.folnum + 1, 1))
            self.traj_pose_z = np.zeros((self.folnum + 1, 1))
            find_index = self.colony_list.index(self.uav_id)
            self.traj_pose_x[find_index,0] = self.uavpos_x[find_index]
            self.traj_pose_y[find_index,0] = self.uavpos_y[find_index]
            self.traj_pose_z[find_index,0] = self.uavpos_z[find_index]

            # for drone_id in range(self.folnum):
            #     self.traj_pose_x[drone_id, 0] = (self.longitude[drone_id] - self.original_position[
            #         0]) / 180 * math.pi * (EARTH_RADIUS + self.original_position[2])
            #     self.traj_pose_y[drone_id, 0] = (self.latitude[drone_id] - self.original_position[
            #         1]) / 180 * math.pi * (EARTH_RADIUS + self.original_position[2])
            #     self.traj_pose_z[drone_id, 0] = self.altitude[drone_id] - self.original_position[2]
            # 发送规划控制点
            rows, cols = self.traj_pose_x.shape
            # 将每个矩阵展平并合并
            data = np.concatenate([
                self.traj_V_x.flatten(),
                self.traj_V_y.flatten(),
                self.traj_V_z.flatten(),
                self.traj_pose_x.flatten(),
                self.traj_pose_y.flatten(),
                self.traj_pose_z.flatten()
            ])

            # 创建 Float64MultiArray 消息
            pose_msg = Float64MultiArray()
            pose_msg.data = data.tolist()

            # 设置 layout，指定行列信息
            pose_msg.layout.dim = [
                MultiArrayDimension(label='row', size=rows, stride=cols * 6),  # 6 个矩阵，stride 是每个矩阵的列数
                MultiArrayDimension(label='column', size=cols, stride=1)
            ]

            self.plan_cmd_pub.publish(pose_msg)
            self.get_logger().info('Publishing stop cmd')
            self.status == "waittarget"

    def checkObstaclescallback(self):
        if self.status =="init":
            return
        # self.get_logger().info("Obstacle update ...")
        # 调用异步服务请求
        request = GetParameters.Request()
        request.names = ['calculated_obstacles']
        future = self.client.call_async(request)
        future.add_done_callback(self.callback_global_param)

        self.filtered_obstacles = []
        if np.any(self.calculated_obstacles):
            # 距离阈值 r
            r = self.detect_R  # 设定一个距离范围

            # 创建 KD-Tree
            kdtree = cKDTree(self.calculated_obstacles)

            find_index = self.colony_list.index(self.uav_id)

            # 读取当前无人机位置 这里要修改为当前无人机的位置
            x0 = self.uavpos_x[find_index]
            y0 = self.uavpos_y[find_index]
            z0 = self.uavpos_z[find_index]
            # print([x0, y0, z0])
            # 进行范围查询，返回在距离阈值 r 范围内的障碍物的索引
            indices = kdtree.query_ball_point([x0, y0, z0], r)
            # print(indices)
            # print(self.calculated_obstacles)
            # print(len(self.calculated_obstacles))
            # 过滤无效索引，确保 indices 中的每个索引都不超出 self.calculated_obstacles 的大小
            # 如果 indices 为空（即没有找到任何障碍物），则跳过扁平化
            if len(indices) > 0:  # 如果是嵌套列表，进行扁平化
                # 确保 indices 中的每个索引都在合法范围内
                indices = np.clip(indices, 0, len(self.calculated_obstacles) - 1)
                # 根据索引获取范围内的障碍物
                self.filtered_obstacles = self.calculated_obstacles[indices]
                # print(self.filtered_obstacles)  
            else:
                self.filtered_obstacles = []  # 如果没有找到障碍物，设为空列表

        for uav_id in self.cluster_list:
            if uav_id not in self.colony_list:
                find_index = self.cluster_list.index(uav_id)
                own_index = self.cluster_list.index(self.uav_id)
                x = self.cluster_uavpos_x[find_index]
                y = self.cluster_uavpos_y[find_index]
                z = self.cluster_uavpos_z[find_index]
                dx = self.cluster_uavpos_x[find_index] - self.cluster_uavpos_x[own_index]
                dy = self.cluster_uavpos_y[find_index] - self.cluster_uavpos_y[own_index]
                dz = self.cluster_uavpos_z[find_index] - self.cluster_uavpos_z[own_index]
                distance = np.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
                if distance < self.detect_R:
                    self.filtered_obstacles.append(np.array([x,y,z]))


        if np.any(self.filtered_obstacles):
            self.status = "replan"

    def checkUavPos(self):
        if self.folnum == 1:
            return
        if self.status == "init":
            return
        if self.status == "stop":
            return
        if self.status == "grouping":
            return
        if self.status == "replan":
            return
        # 读取当前无人机位置 这里要修改为当前无人机的位置
        find_index = self.colony_list.index(self.uav_id)#找出当前无人机对应的序号
        for i in range(self.folnum):
            if i != find_index:
                if i < len(self.timestamps) and self.timestamps[i] is not None:
                    #飞大飞机时，要确定是否需要高度，现在无人机高度漂移太大
                    distance = np.sqrt(
                        (self.uavpos_x[find_index] - self.uavpos_x[i]) ** 2 + (self.uavpos_y[find_index] - self.uavpos_y[i]) ** 2 + (
                                    self.uavpos_z[find_index] - self.uavpos_z[i]) ** 2)
                    # distance = np.sqrt(
                    #     (self.uavpos_x[find_index] - self.uavpos_x[i]) ** 2 + (self.uavpos_y[find_index] - self.uavpos_y[i]) ** 2 )
                    if distance < self.min_distance:
                        self.min_distance = distance
                        self.get_logger().info(f"Drone {self.colony_list[i]} and Drone {self.uav_id} :{distance}m")

                    # # 如果两架无人机的距离小于 2 米，则发送停止信号
                    # if distance < 5.0:
                    #     self.get_logger().info(f"Sending stop signal: Drone {self.colony_list[i]} and Drone {self.uav_id} are too close!")
                    #     print(f"uavposx={self.uavpos_x},uavposy={self.uavpos_y},uavposz={self.uavpos_z}")
                    #     self.status = "stop"

    def vehicle_scan_callback(self, vehicle_scan):
        cloud_out = self.laserProj.projectLaser(vehicle_scan)
        self.laser_to_pointcloud_publisher.publish(cloud_out)

    def callback_global_param(self, future):
        try:
            result = future.result()
        except Exception as e:
            self.get_logger().warn("service call failed %r" % (e,))
        else:
            param = result.values[0]
            # print(param)
            data = param.double_array_value.tolist()
            # print(data)
            # 确保列表的长度为 3 的倍数
            if len(data) % 3 != 0:
                data = data[:(len(data) // 3) * 3]
            # 将处理后的列表转换为二维数组，每行包含 3 个元素
            self.calculated_obstacles = np.array(data).reshape(-1, 3)
            # self.get_logger().info("Got obstacles param: %s" % (self.calculated_obstacles,))

    def callback_global_param_from_goal_node(self, future):
        try:
            result = future.result()
        except Exception as e:
            self.get_logger().warn("service call failed %r" % (e,))
        else:
            param = result.values[0]
            # print(param)
            data = param.double_array_value.tolist()
            # print(data)
            # 确保列表的长度为 3 的倍数
            if len(data) % 3 != 0:
                data = data[:(len(data) // 3) * 3]

            print(f"original_position:{self.original_position}")

            for index in range(0, len(data), 3):
                data[index] = (data[index]-self.original_position[0])/180*math.pi*(EARTH_RADIUS + self.original_position[2])
                data[index+1] = (data[index+1]-self.original_position[1])/180*math.pi*(EARTH_RADIUS + self.original_position[2])
                data[index+2] = data[index+2] - self.original_position[2]
            # 将处理后的列表转换为二维数组，每行包含 3 个元素
            self.calculated_obstacles = np.array(data).reshape(-1, 3)
            print(self.calculated_obstacles)
            self.get_logger().info("Got obstacles param: %s" % (self.calculated_obstacles,))

    
    def formation_control(self, form_id, dis):
        form_id = form_id - 1
        delta_x = []
        delta_y = []
        delta_z = []
        # init_f = np.array([[5.0, 5.0, 0.0],[5.0, 0.0, 0.0], [5.0, -5.0, 0.0], [-2.0,0.0,0.0]])
        dist_matrix = np.zeros((self.folnum, self.folnum), dtype=float)
        diff_init = []
        form_x = []
        form_y = []
        form_z = []
        for i in range(self.folnum + 1):
            # diff_init.append(self.init_f[i][:3] - self.init_f[self.folnum][:3])
            diff_init.append(self.init_f[i][:3])

        formation_lists = []
        if form_id == 0:
            for i in range(self.folnum):
                form_x.append((-1)**(i+1) * ((i+1) // 2)*dis)
                form_y.append(0.0)
                form_z.append(0.0)
        elif form_id == 1:
            for i in range(self.folnum):
                form_x.append(0.0)
                form_y.append((-1)**(i+1) * ((i+1) // 2)*dis)
                form_z.append(0.0)
        elif form_id == 2:
            remaining_points = self.folnum
            row = 1
            while remaining_points > 0:
                num_points_in_row = row

                if remaining_points < num_points_in_row:
                    num_points_in_row = remaining_points
                for i in range(num_points_in_row):
                    form_x.append((1-row)*0.5*dis+i*dis)
                    form_y.append((1-row)*0.5*(3.0 ** 0.5)*dis)
                    form_z.append(0.0)
                remaining_points -= num_points_in_row
                row += 1
        #return to home formation
        elif form_id == 3:
            for i in range(self.folnum):
                X = (self.return_position[i][0] - self.original_position[0]) / 180 * math.pi * (
                        EARTH_RADIUS + self.original_position[2])
                Y = (self.return_position[i][1] - self.original_position[1]) / 180 * math.pi * (
                        EARTH_RADIUS + self.original_position[2])
                Z = self.return_position[i][2] - self.original_position[2]
                form_x.append(X)
                form_y.append(Y)
                form_z.append(Z)
        leader_x = sum(form_x)/len(form_x)
        leader_y = sum(form_y)/len(form_y)
        leader_z = sum(form_z)/len(form_z)

        for i in range(self.folnum):
            formation_lists.append([form_x[i]-leader_x, form_y[i]-leader_y, form_z[i]-leader_z])
        formation_lists.append([0.0, 0.0, 0.0])


        for i in range(self.folnum):
            for j in range(self.folnum):
                # print(f"len:fadishjfkjhjdk:::::{len(formation_lists[j])}")
                # print(f"len:fadishjfkjhjdk:::::{len(self.goal)}")
                dist_matrix[i][j] = norm(diff_init[i], [formation_lists[j][0]+self.goal[0],formation_lists[j][1]+self.goal[1],formation_lists[j][2]+self.goal[2]])
        # print(f"dist_matrix:{dist_matrix}")
        row_ind, col_ind = linear_sum_assignment(dist_matrix)
        matching = list(zip(row_ind, col_ind))
        # print(f"matching:{matching}")
        for i, j in matching:
            # for j in range(self.folnum):
            # diff_init[i] = np.array(formation_lists[j])
            delta_x.append(formation_lists[j][0])
            delta_y.append(formation_lists[j][1])
            delta_z.append(formation_lists[j][2])
        delta_x.append(0.0)
        delta_y.append(0.0)
        delta_z.append(0.0)
        # for i in range(self.folnum+1):
        # diff_init.append(init_f[i][:3]-init_f[self.folnum][:3])
        return np.array(delta_x), np.array(delta_y), np.array(delta_z)



    def compute_repulsion(self, robot_pose, obs_pose, detect_R):
        '''计算斥力函数'''
        repulsion = np.array([0.0, 0.0, 0.0])  # 不改变z轴的值
        # safe_dis = 25.0
        temp = 0.0
        for obs in obs_pose:
            distance = np.sqrt((robot_pose[0] - obs[0]) ** 2 + (
                    robot_pose[1] - obs[1]) ** 2)  # 计算当前无人机和障碍物的平面距离（点云里怎么获取这个obs_pose，现在还都是自己设定的）

            # 检查距离是否为零，避免除以零
            if distance == 0.0:
                continue  # 跳过当前障碍物
            # if distance >4:
            #     distance-=4
            # elif distance>3 and distance <=4 :
            #     distance-=3
            if distance <= self.safe_distance:
                temp = 99999999.0
            elif distance <= detect_R:
                temp = (1 / distance - 1 / detect_R) / (distance ** 2)  # 计算斥力大小，用经验推导的斥力函数
                # 检查临时变量是否为无效值，避免产生无效值
            if not np.isnan(temp) and not np.isinf(temp):
                repulsion[0] += temp * (robot_pose[0] - obs[0])
                repulsion[1] += temp * (robot_pose[1] - obs[1])
                # repulsion[2] += temp * (robot_pose[2] - obs[2])
        return repulsion

    def confine(self, current, next, Kinematic, dt):
        ''' 基于运动学给无人机速度做约束，给定的最大速度和加速度限制'''
        # 机器人的动力学参数：最大响应距离：m，速度：m/s，平面加速度：m/s^2，垂直加速度：m/s^2 和 rad/s^2
        # Kinematic = np.array([5.0, 5.0, 3.0, 4.0, 0.5, 16.0, 0.5])
        # Limitation on speed x,y
        delta_x = next[0] - current[0]  # 计算无人机在 x 方向上的速度变化量
        delta_y = next[1] - current[1]
        delta_z = next[2] - current[2]
        delta_norm = np.sqrt(delta_x ** 2 + delta_y ** 2)
        # print(f"delta_x: {delta_x}, delta_y: {delta_y}, delta_norm: {delta_norm}")
        if delta_norm != 0.0:
            allF_x = delta_x / delta_norm
            allF_y = delta_y / delta_norm
        else:
            allF_x = 0.0
            allF_y = 0.0
                # print(f"Warning: delta_norm is zero. current0: {current[0.0]}   current1: {current[1]}")
            # print("Warning: Invalid delta_x or delta_y value (NaN or Inf).")
        # print(f"current[0]: {current[0]} current[1]: {current[1]} current[2]: {current[2]}")
        # print(f"allF_x: {allF_x}  allF_y: {allF_y}")
        if delta_x >= 0.0:
            next[0] = min(next[0], current[0] + Kinematic[2] * allF_x * dt)
        else:
            next[0] = max(next[0], current[0] + Kinematic[2] * allF_x * dt)
        # Limitation on speed y
        if delta_y >= 0.0:
            next[1] = min(next[1], current[1] + Kinematic[2] * allF_y * dt)
        else:
            next[1] = max(next[1], current[1] + Kinematic[2] * allF_y * dt)
        # Limitation on speed z
        if delta_z >= 0.0:
            next[2] = min(next[2], current[2] + Kinematic[3] * dt)
        else:
            next[2] = max(next[2], current[2] - Kinematic[3] * dt)
        # print(f"current[0]: {current[0]} current[1]:{current[1]} current[2]: {current[2]}")
        # print(f"next[0]: {next[0]} next[1]:{next[1]} next[2]: {next[2]}")
        return next

    def yaw_to_orientation(self, yaw):  # yaw角转换为orientation
        # Convert yaw angle in radians to quaternion
        yaw -= 1.57
        quaternion = R.from_euler('z', yaw).as_quat()
        return quaternion

    def consensus_APF_command_callback(self, msg):

        self.sendStatus(2)
        self.status_return_flag = True
        # 命令接收，包含队形、目标点、命令
        if self.status == 'stop':
            self.status = "stop"
        elif self.status == "init":
            self.status = "init"
        else:
            self.status = "replan"
        if msg.frame_id =="grouping":
            # print(msg)
            self.clusterId = msg.clusterid
            self.childClusterId = msg.childclusterid
            self.childCluster_list = msg.dynamic_array
            self.colony_list = msg.dynamic_array
            lon = msg.pose.position.x
            lat = msg.pose.position.y
            alt = msg.pose.position.z
            self.goal[0] = (lon - self.original_position[0]) / 180 * math.pi * (
                        EARTH_RADIUS + self.original_position[2])
            self.goal[1] = (lat - self.original_position[1]) / 180 * math.pi * (
                    EARTH_RADIUS + self.original_position[2])
            self.goal[2] = alt - self.original_position[2]

            own_lon = msg.pose.orientation.x
            own_lat = msg.pose.orientation.y
            own_alt = msg.pose.orientation.z
            self.task_goal[0] = (own_lon - self.original_position[0]) / 180 * math.pi * (
                        EARTH_RADIUS + self.original_position[2])
            self.task_goal[1] = (own_lat - self.original_position[1]) / 180 * math.pi * (
                    EARTH_RADIUS + self.original_position[2])
            self.task_goal[2] = own_alt - self.original_position[2]

            # 输出数组，查看提取的非零值
            self.get_logger().info(
                f'Received cluster{self.clusterId}: childCluster{self.childClusterId}: {self.colony_list}'
                f'target:[{self.goal[0]},{self.goal[1]},{self.goal[2]}]')
            if self.task_planning_flag:
                self.status = "task"
            else:
                self.status = "grouping"
            if lon == 1000.0 and lat == 1000.0 and alt == 1000.0:
                self.status = "return_to_home"
        elif msg.frame_id =="return_to_home":
            if msg.clusterid == self.clusterId:               
                # find_index = self.cluster_list.index(self.colony_list[0])
                # find_index = 0
                # self.goal[0] = (self.return_position[find_index][0] - self.original_position[0]) / 180 * math.pi * (
                #         EARTH_RADIUS + self.original_position[2])
                # self.goal[1] = (self.return_position[find_index][1] - self.original_position[1]) / 180 * math.pi * (
                #         EARTH_RADIUS + self.original_position[2])
                # self.goal[2] = self.return_position[find_index][2] - self.original_position[2]
                self.status = "return_to_home"
            else:
                print(f"Not belong to cluster{msg.clusterid}")
        elif msg.frame_id =="stop":
            self.status = "stop"
        elif msg.frame_id == "cluster": #初始化大集群
            self.cluster_flag = True
            self.clusterId = msg.clusterid
            self.childClusterId = 0
            self.camp = msg.camp
            self.formation = msg.formation
            self.interval = msg.interval
            self.safe_distance = msg.safe_distance
            self.detect_R = self.safe_distance + 10.0
            self.cluster_veclocity = msg.cluster_velocity
            self.cluster_list = msg.dynamic_array
            self.colony_list = msg.dynamic_array
            self.task_exec_flag = [False] * len(self.cluster_list)
            # 输出数组，查看提取的非零值
            self.get_logger().info(
                f'Received cluster info: {msg}')
            self.status = "grouping"
        elif msg.frame_id == "task":  #到达目标区域执行任务
            self.status = "task"
        elif msg.frame_id == "formation":
            if msg.clusterid == self.clusterId:
                self.formation = msg.formation
                self.colony_list = self.childCluster_list
                # self.interval = msg.interval
                # if msg.pose.position.z != -1.0:   # 命令中默认包含0，0，0点，设置一个无效标志值
                # self.goal[0] = msg.pose.position.x
                # self.goal[1] = msg.pose.position.y
                # self.goal[2] = msg.pose.position.z
                self.get_logger().info(
                    f'Received formation_id: {msg.formation},'f'Received PoseStamped: Position({self.goal[0]}, {self.goal[1]}, {self.goal[2]})')
            else:
                print(f"Not belong to cluster{msg.clusterid}")
        


    def consensus_APF(self, pose_x, pose_y, pose_z, pose_th,V_x,V_y,V_z, goal, ob_temp, A, delta_x, delta_y, delta_z, countmax,
                      fol_num, N, detect_R, Kinematic, dt, k):
        '''一致性编队+人工势场法避障'''

        count = 0
        KN = 1  # KN的值需要根据实际情况设定,吸引力增益,增大KN会使无人机更快地向目标点靠近  1
        beta = 20  # beta的值需要根据实际情况设定,排斥力增益,增大beta能增强无人机避开障碍的能力  5
        # d_max = 4  # d_max的值需要根据实际情况设定,无人机对目标点或其他无人机的最大响应距离
        gama = 0.5  # gama的值需要根据实际情况设定,一致性增益 0.37
        K0 = 0.25  # K0的值需要根据实际情况设定,速度缩放因子 0.37

        # V_x = np.zeros((N, 2))  # 无人机集群x方向的速度
        # V_y = np.zeros((N, 2))  # 无人机集群y方向的速度
        # V_z = np.zeros((N, 2))  # 无人机集群y方向的速度
        # has_obs = 0
        # V_max = 0.5
        # 设定一个阈值epsilon，表示目标点足够接近时认为到达
        epsilon = 0.5
        # while count < countmax:  # 计算固定点数
        while True:
            if self.formation == 0:  # 保持当前队形
                delta_x = self.delta_x
                delta_y = self.delta_y
                delta_z = self.delta_z
            else: 
                delta_x, delta_y, delta_z = self.formation_control(self.formation, self.interval)
            end_point_x = self.goal[0]
            end_point_y = self.goal[1]
            end_point_z = self.goal[2]
            goal = (end_point_x, end_point_y, end_point_z)  # 修改目标点位置
            count += 1
            k += 1
            V_x = np.column_stack((V_x, np.zeros(V_x.shape[0])))
            V_y = np.column_stack((V_y, np.zeros(V_y.shape[0])))
            V_z = np.column_stack((V_z, np.zeros(V_z.shape[0])))
            distance = np.sqrt((goal[0] - pose_x[N - 1, k]) ** 2 + (goal[1] - pose_y[N - 1, k]) ** 2 + (
                        goal[2] - pose_z[N - 1, k]) ** 2)
            # distance_xy = np.sqrt((goal[0] - pose_x[N - 1, k]) ** 2 + (goal[1] - pose_y[N - 1, k]) ** 2)
            delta = np.array([goal[0] - pose_x[N - 1, k], goal[1] - pose_y[N - 1, k], goal[2] - pose_z[N - 1, k]])

            # th = np.arctan2(goal[1] - pose_y[N - 1, k], goal[0] - pose_x[N - 1, k])
            # gam = np.arctan2(goal[2] - pose_z[N - 1, k], distance_xy)
            # if distance > d_max:
            # distance = d_max
            # 根据当前领导者和目标距离和角度计算领导者速度
            # V_x[N - 1, k + 1] = KN * distance * np.cos(gam) * np.cos(th)
            # V_y[N - 1, k + 1] = KN * distance * np.cos(gam) * np.sin(th)
            # V_z[N - 1, k + 1] = KN * distance * np.sin(gam)

            # 根据雷达获取的障碍物位置更新ob_pose，用get_param
            if np.any(self.filtered_obstacles):  # 判断self.calculated_obstacles是否非空
                ob_temp = self.filtered_obstacles
            ob_pose = ob_temp
            
            repulsion = self.compute_repulsion([pose_x[N - 1, k], pose_y[N - 1, k], pose_z[N - 1, k]], ob_pose,
                                               detect_R)  # 计算平面内的障碍物的斥力

            # 判断是否已经到达目标点
            if distance < 0.5:
                V_x[N - 1, k + 1] = 0  # 更新速度
                V_y[N - 1, k + 1] = 0
                V_z[N - 1, k + 1] = 0
            else:
                # 将障碍物斥力加到领导者速度上
                previous_velocity = np.array([V_x[N - 1, k], V_y[N - 1, k], V_z[N - 1, k]])  # 计算之前的速度
                if distance > Kinematic[0]:
                    allF_delta = delta / distance
                    delta[0] = Kinematic[0] * allF_delta[0]
                    delta[1] = Kinematic[0] * allF_delta[1]
                    delta[2] = Kinematic[0] * allF_delta[2]
                numerator = K0 * previous_velocity + gama * delta + beta * repulsion  # 计算 allF
                denominator = np.linalg.norm(numerator)
                if denominator > Kinematic[1]:
                    allF = numerator / denominator
                    V_x[N - 1, k + 1] = Kinematic[1] * allF[0]  # 更新速度   #可能需要修改
                    V_y[N - 1, k + 1] = Kinematic[1] * allF[1]
                    V_z[N - 1, k + 1] = Kinematic[1] * allF[2]
                else:
                    V_x[N - 1, k + 1] = numerator[0]  # 更新速度
                    V_y[N - 1, k + 1] = numerator[1]
                    V_z[N - 1, k + 1] = numerator[2]

                if (distance > 1 and abs(V_x[N - 1, k + 1]) <= 0.1 and abs(V_y[N - 1, k + 1]) <= 0.1 and abs(
                        V_z[N - 1, k + 1]) <= 0.1):
                    V_x[N - 1, k + 1] += -1 + 2 * np.random.rand()  # 如果领导者速度很小，且还没到目标，给个随机速度
                    V_y[N - 1, k + 1] += -1 + 2 * np.random.rand()
                    V_z[N - 1, k + 1] += -1 + 2 * np.random.rand()
                out = self.confine([V_x[N-1, k], V_y[N-1, k],V_z[N - 1, k]], [V_x[N - 1, k + 1], V_y[N - 1, k + 1],V_z[N - 1, k + 1]], Kinematic, 0.1)
                V_x[N - 1, k+1] = out[0]
                V_y[N - 1, k+1] = out[1]
                V_z[N - 1, k+1] = out[2]
                

            for i in range(0, fol_num):  # 跟随者位置计算
                sum_delta_x = 0
                sum_delta_y = 0
                sum_delta_z = 0
                for j in range(0, N):
                    if A[i, j] == 1:
                        w_ij = 2 - np.exp(-((pose_x[j, k] - pose_x[i, k] - (delta_x[j] - delta_x[i])) ** 2 + (
                                    pose_y[j, k] - pose_y[i, k] - (delta_y[j] - delta_y[i])) ** 2 + (
                                                        pose_z[j, k] - pose_z[i, k] - (delta_z[j] - delta_z[i])) ** 2))
                        sum_delta_x += A[i, j] * w_ij * ((pose_x[j, k] - pose_x[i, k]) - (delta_x[j] - delta_x[i]))
                        sum_delta_y += A[i, j] * w_ij * ((pose_y[j, k] - pose_y[i, k]) - (delta_y[j] - delta_y[i]))
                        sum_delta_z += A[i, j] * w_ij * ((pose_z[j, k] - pose_z[i, k]) - (delta_z[j] - delta_z[i]))
                distance = np.sqrt(sum_delta_x ** 2 + sum_delta_y ** 2 + sum_delta_z ** 2)
                # 判断是否已经到达目标点
                # print(f"{i}-----")
                # print(distance)
                if distance < epsilon:
                    V_x[i, k + 1] = V_x[N - 1, k+1]  # 更新速度
                    V_y[i, k + 1] = V_y[N - 1, k+1]
                    V_z[i, k + 1] = V_z[N - 1, k+1]
                    # if i == self.uav_id-1:
                    self.arrive_flag[i] = True
                    # print("跟随者到达位置")
                else:
                    # distance_xy = np.sqrt(sum_delta_x**2 + sum_delta_y**2)
                    # th = np.arctan2(sum_delta_y, sum_delta_x)
                    # gam = np.arctan2(sum_delta_z, distance_xy)
                    reference_velocity = np.array([V_x[N - 1, k+1], V_y[N - 1, k+1], V_z[N - 1, k+1]])  # 计算虚拟leader的速度
                    delta = np.array([sum_delta_x, sum_delta_y, sum_delta_z])

                    # V_x[i, k + 1] = KN * V_x[N-1, k] + gama * distance * np.cos(gam) * np.cos(th) #计算跟随者的下一时刻速度
                    # V_y[i, k + 1] = KN * V_y[N-1, k] + gama * distance * np.cos(gam) * np.sin(th)
                    # V_z[i, k + 1] = KN * V_z[N-1, k] + gama * distance * np.sin(gam)
                    # out = self.confine([V_x[i, k], V_y[i, k]], [V_x[i, k+1], V_y[i, k+1]], Kinematic, 0.1)
                    # V_x[i, k+1] = out[0]
                    # V_y[i, k+1] = out[1]
                    kk = 0
                    # obs_pose = np.zeros((N - 1, 3))
                    for j in range(0, N - 1):  # 计算和其他无人机之间的斥力
                        if j != i:
                            obs_pose = np.zeros((1, 3))
                            kk += 1
                            # obs_pose[kk - 1, 0] = pose_x[j, k]
                            # obs_pose[kk - 1, 1] = pose_y[j, k]
                            # obs_pose[kk - 1, 2] = pose_z[j, k]
                            obs_pose[0, 0] = pose_x[j, k]
                            obs_pose[0, 1] = pose_y[j, k]
                            obs_pose[0, 2] = pose_z[j, k]
                            if np.any(ob_pose):
                                ob_pose = np.vstack((ob_pose, obs_pose))
                            else:
                                ob_pose = obs_pose

                    # print(f"ob_pose{ob_pose}")
                    repulsion = self.compute_repulsion([pose_x[i, k], pose_y[i, k], pose_z[i, k]], ob_pose, detect_R)
                    if distance > Kinematic[0]:
                        allF_delta = delta / distance
                        delta[0] = Kinematic[0] * allF_delta[0]
                        delta[1] = Kinematic[0] * allF_delta[1]
                        delta[2] = Kinematic[0] * allF_delta[2]
                    numerator = KN * reference_velocity + gama * delta + beta * repulsion
                    denominator = np.linalg.norm(numerator)
                    if denominator > Kinematic[1]:    #对速度大小进行限制
                        allF = numerator / denominator
                        V_x[i, k + 1] = Kinematic[1] * allF[0]  # 更新速度
                        V_y[i, k + 1] = Kinematic[1] * allF[1]
                        V_z[i, k + 1] = Kinematic[1] * allF[2]
                    else:
                        V_x[i, k + 1] = numerator[0]  # 更新速度
                        V_y[i, k + 1] = numerator[1]
                        V_z[i, k + 1] = numerator[2]
                    if (distance > 1 and abs(V_x[i, k + 1]) <= 0.1 and abs(V_y[i, k + 1]) <= 0.1 and abs(
                            V_z[i, k + 1]) <= 0.1):
                        V_x[i, k + 1] += -1 + 2 * np.random.rand()  # 如果速度很小，且还没到目标，给个随机速度
                        V_y[i, k + 1] += -1 + 2 * np.random.rand()
                        V_z[i, k + 1] += -1 + 2 * np.random.rand()
                    out = self.confine([V_x[i, k], V_y[i, k],V_z[i, k]], [V_x[i, k + 1], V_y[i, k + 1],V_z[i, k + 1]], Kinematic, 0.1)
                    V_x[i, k+1] = out[0]
                    V_y[i, k+1] = out[1]
                    V_z[i, k+1] = out[2]
                    self.arrive_flag[i] = False
            pose_xtemp = np.zeros((N, 1))
            pose_ytemp = np.zeros((N, 1))
            pose_ztemp = np.zeros((N, 1))
            pose_thtemp = np.zeros((N, 1))
            for i in range(0, N):
                # out = self.confine([V_x[i, k], V_y[i, k]], [V_x[i, k+1], V_y[i, k+1]], Kinematic, 0.1)
                # V_x[i, k+1] = out[0]
                # V_y[i, k+1] = out[1]
                pose_xtemp[i, 0] = pose_x[i, k] + dt * V_x[i, k + 1]  # 根据当前位置和速度计算下一时刻的位置
                pose_ytemp[i, 0] = pose_y[i, k] + dt * V_y[i, k + 1]  # y and th error
                pose_ztemp[i, 0] = pose_z[i, k] + dt * V_z[i, k + 1]
                pose_thtemp[i, 0] = np.arctan2(V_y[i, k + 1], V_x[i, k + 1])  # 计算yaw角
               
            pose_x = np.column_stack((pose_x, pose_xtemp))
            pose_y = np.column_stack((pose_y, pose_ytemp))
            pose_z = np.column_stack((pose_z, pose_ztemp))
            pose_th = np.column_stack((pose_th, pose_thtemp))

            # print(f"我在这里  x: {pose_xtemp}   y: {pose_ytemp}  z: {pose_ztemp}")
            # print(f"我的速度  Vx: {V_x[:, k + 1]}   Vy: {V_y[:, k + 1]}  Vz: {V_z[:, k + 1]}")
            
            # 判断是否到达目标
            difference_now = np.sqrt((goal[0] - pose_x[N - 1, k + 1]) ** 2 + (goal[1] - pose_y[N - 1, k + 1]) ** 2 + (
                        goal[2] - pose_z[N - 1, k + 1]) ** 2)
            # print("------")
            # print(self.arrive_flag)
            # print(all(self.arrive_flag))
            if (difference_now < 0.5 and all(self.arrive_flag)) or count ==countmax:
            # if difference_now < 0.5 and all(self.arrive_flag):
                print('Complete the planning!!')  # 如果领导者到达目标点附近，则显示到达信息并结束循环
                self.traj_k = k + 1
                self.traj_V_x = V_x
                self.traj_V_y = V_y
                self.traj_V_z = V_z
                self.traj_pose_x = pose_x
                self.traj_pose_y = pose_y
                self.traj_pose_z = pose_z
                # 凌云
                # self.longitude[7] = self.original_position[0] + (self.traj_pose_x[7, self.traj_k] / (
                #             EARTH_RADIUS + self.original_position[2])) * 180 / math.pi
                # self.latitude[7] = self.original_position[1] + (self.traj_pose_y[7, self.traj_k] / (
                #             EARTH_RADIUS + self.original_position[2])) * 180 / math.pi
                # self.altitude[7] = self.original_position[2] + self.traj_pose_z[7, self.traj_k]
                    # self.timestamps[uav_i] = 1.0  # 记录时间戳
                print(f"x: {self.traj_pose_x}  lenth: {len(self.traj_pose_x[0])}")
                print(f"y: {self.traj_pose_y}  lenth: {len(self.traj_pose_y[0])}")
                print(f"z: {self.traj_pose_z}  lenth: {len(self.traj_pose_z[0])}")
                # print(f"Vx: {self.traj_V_x}  lenth: {len(self.traj_V_x[0])}")
                # print(f"Vy: {self.traj_V_y}  lenth: {len(self.traj_V_y[0])}")
                # print(f"Vz: {self.traj_V_z}  lenth: {len(self.traj_V_z[0])}")
                break

    def pose_callback(self, msg, uav_id):
        # self.get_logger().info(f"Drone_{drone_id+1} is getting position!")
        # 从消息中读取位置数据
        # 与数仿调试
        # if self.flag:
        #     if drone_id ==7:
        #         return
        try:
            # print(f"colony_list={self.colony_list}")
            find_index = self.cluster_list.index(uav_id)
            self.cluster_longitude[find_index] = msg.data[1]  # 假设单位为度*1E7
            self.cluster_latitude[find_index] = msg.data[2]
            self.cluster_altitude[find_index] = msg.data[3]
            self.cluster_timestamps[find_index] = msg.data[0]  # 记录时间戳
            self.cluster_uavpos_x[find_index] = (self.cluster_longitude[find_index] - self.original_position[0]) / 180 * math.pi * (
                        EARTH_RADIUS + self.original_position[2])
            self.cluster_uavpos_y[find_index] = (self.cluster_latitude[find_index] - self.original_position[1]) / 180 * math.pi * (
                        EARTH_RADIUS + self.original_position[2])
            self.cluster_uavpos_z[find_index] = self.cluster_altitude[find_index] - self.original_position[2]
            if uav_id in self.colony_list:
                index = self.colony_list.index(uav_id)
                self.longitude[index] = msg.data[1]  # 假设单位为度*1E7
                self.latitude[index] = msg.data[2]
                self.altitude[index] = msg.data[3]
                self.uav_velocity_x[index] = msg.data[4]
                self.uav_velocity_y[index] = msg.data[5]
                self.uav_velocity_z[index] = msg.data[6]
                self.timestamps[index] = msg.data[0]  # 记录时间戳
                self.uavpos_x[index] = (self.longitude[index] - self.original_position[0]) / 180 * math.pi * (
                            EARTH_RADIUS + self.original_position[2])
                self.uavpos_y[index] = (self.latitude[index] - self.original_position[1]) / 180 * math.pi * (
                            EARTH_RADIUS + self.original_position[2])
                self.uavpos_z[index] = self.altitude[index] - self.original_position[2]
                if self.task_goal[0]!=None and self.task_goal[1]!=None and self.task_goal[2]!=None:
                    dx = self.uavpos_x[index] - self.task_goal[0]
                    dy = self.uavpos_y[index] - self.task_goal[1]
                    dz = self.uavpos_z[index] - self.task_goal[2]
                    distance = np.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
                    if distance < 50.0:
                        self.task_exec_flag[find_index] = True
                    sum = 0
                    for uav_index in self.colony_list:
                        find_index = self.cluster_list.index(uav_index)
                        sum += self.task_exec_flag[find_index]
                    if sum == len(self.colony_list):
                        self.task_planning_flag=True
                    else:
                        self.task_planning_flag = False


            # print(f"get uav{uav_id} lon: {self.longitude[find_index]}--lat: {self.latitude[find_index]}--alt: {self.altitude[find_index]}")
            # print(f"get uav{uav_id} x: {self.uavpos_x[find_index]}--y: {self.uavpos_y[find_index]}--z: {self.uavpos_z[find_index]}")
        except Exception as e:
            # print(f"uav_id{uav_id},,find_index{find_index},,index{index},,,\ncluster_list{self.cluster_list}\ncolony_list{self.colony_list}")
            self.get_logger().warn(f"service call failed %r" % (e,))
        
    def update_subscriptions(self, new_active_topics):
        """
        更新订阅：根据 `new_active_topics` 列表动态添加和删除订阅。
        new_active_topics: 需要订阅的活跃话题的索引列表
        """
        # 取消所有旧订阅
        for drone_pose_sub in self.drones_pose_sub:
            self.destroy_subscription(drone_pose_sub)

        # 清空现有订阅列表
        self.drones_pose_sub.clear()

        qos_profile_1 = rclpy.qos.QoSProfile(
            reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
            history=rclpy.qos.HistoryPolicy.KEEP_LAST,
            depth=10
        )
        # 创建新的订阅
        for i in new_active_topics:
            topic_name = "/drone{}/velocity_and_position".format(i)
            self.get_logger().info(f"Listening to {topic_name}")
            drone_pose_sub = self.create_subscription(
                Float64MultiArray,
                topic_name,
                lambda msg, id=i: self.pose_callback(msg, id),
                qos_profile_1
            )
            self.drones_pose_sub.append(drone_pose_sub)

    def perform_consistency_algorithm(self):
        # self.get_logger().info("perform_consistency_algorithm")

        N = self.folnum + 1  # 机器人个数（跟随者和虚拟领导者）
        countmax = 40  # 最大循环次数
        dt = 0.1  # 控制和通信周期

        k = 0  # 迭代次数
        # 机器人的动力学参数：最大响应距离：m，速度：m/s，平面加速度：m/s^2，垂直加速度：m/s^2
        Kinematic = np.array([5.0, 5.0, 5.0, 5.0])
        # Kinematic = np.array([10.0, 10.0, 3.0, 4.0])


        pose_x = self.init_f[:, 0]  # 从init_f提取所有机器人的x坐标
        pose_y = self.init_f[:, 1]  # 从init_f提取所有机器人的y坐标
        pose_z = self.init_f[:, 2]  # 从init_f提取所有机器人的z坐标
        pose_th = self.init_f[:, 3]  # 从init_f提取所有机器人的朝向角度

        V_x = self.init_v[:, 0]
        V_y = self.init_v[:, 1]  # 无人机集群y方向的速度
        V_z = self.init_v[:, 2]  # 无人机集群y方向的速度

        # print()

        pose_x = np.column_stack((self.init_f[:, 0], pose_x))  # 创建一个二维数组，第一列是初始x坐标，第二列是预留空间
        pose_y = np.column_stack((self.init_f[:, 1], pose_y))  # 创建一个二维数组，第一列是初始y坐标，第二列是预留空间
        pose_z = np.column_stack((self.init_f[:, 2], pose_z))  # 创建一个二维数组，第一列是初始z坐标，第二列是预留空间
        pose_th = np.column_stack((self.init_f[:, 3], pose_th))  # 创建一个二维数组，第一列是初始朝向角度，第二列是预留空间

        V_x = np.column_stack((self.init_v[:, 0], V_x))  
        V_y = np.column_stack((self.init_v[:, 1], V_y))  
        V_z = np.column_stack((self.init_v[:, 2], V_z)) 
        # 障碍物的位置
        ob_temp = np.array([])
        # ob_temp = np.array([[40, 40, 40],
        # [5, 7, 4],
        # [4, 11, 4],
        # [5, 10, 4],
        # [10, 18, 4]])

        # 共识算法中的相对位置 (在这里设置队形)
        # V_x = np.zeros((N, 1))  # 初始化速度矩阵x分量
        # V_y = np.zeros((N, 1))  # 初始化速度矩阵y分量
        # d_max = 2  # 最大距离

        self.consensus_APF(pose_x, pose_y, pose_z, pose_th,V_x,V_y,V_z, self.goal, ob_temp, self.A, self.delta_x, self.delta_y,
                           self.delta_z, countmax, self.folnum, N, self.detect_R,
                           Kinematic, dt, k)


    def generate_A(self,n,flag):
        '''n表示生成矩阵维度
           flag表示生成邻接矩阵的类型，1：全为1；0：最后一行和最后一列为1'''
        
        if flag == 0:
            atric_M = np.zeros((n, n),dtype=int)
            atric_M[-1,:] = 1
            atric_M[:,-1] = 1
        elif flag == 1:
            atric_M = np.ones((n, n),dtype=int)

        return atric_M
def main(args=None):
    # 初始化 ROS 2 客户端库
    rclpy.init(args=args)

    # 创建并运行 consensus_APF_uav 实例
    node = consensus_APF_uav()

    # 使用 MultiThreadedExecutor 来支持并行回调
    # executor = MultiThreadedExecutor()
    # executor.add_node(node)

    # 让节点保持运行并处理回调函数
    # executor.spin()
    rclpy.spin(node)

    # 销毁节点并关闭 ROS 2 客户端
    # executor.shutdown()
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
